function MoveAllServos(pos_1, pos_2, pos_3, speed)

%     addpath('C:\Users\Pulith Widana\Documents\MATLAB\Dynamixel-AX12-Matlab');
%     addpath('C:\Users\Pulith Widana\Documents\MATLAB\Dynamixel-AX12-Matlab\import');
%     addpath('C:\Users\Pulith Widana\Documents\MATLAB\Dynamixel-AX12-Matlab\bin');
%     %Load the dynamixel library
%     loadlibrary('dynamixel','dynamixel.h');
    DEFAULT_PORTNUM = SetPort;  % COM Channel
    DEFAULT_BAUDNUM = 1;  % 1Mbps
    
    response = calllib('dynamixel','dxl_initialize',DEFAULT_PORTNUM ,DEFAULT_BAUDNUM);
    pause on
    

    
    %There should only be 2 Dynamixels total
    %numberOfDynamixels = length(id)
    
    if response == 1
        
        
        %Broadcast ID
        calllib('dynamixel','dxl_set_txpacket_id', 254);
        
        %Length is 14
        %That handles position and speed for two dynamixels
        calllib('dynamixel','dxl_set_txpacket_length',19);
        
        %SyncWrite instruction
        calllib('dynamixel','dxl_set_txpacket_instruction',131);
        
        %Starting address (goal position)
        calllib('dynamixel','dxl_set_txpacket_parameter',0, 30);
        
        %length of data to write to each dynamixel
        %We're writing position and speed = 4 bytes
        calllib('dynamixel','dxl_set_txpacket_parameter',1, 4);
        
        %Parameters for syncwrite
        % id | position | speed
        %ID 
        calllib('dynamixel','dxl_set_txpacket_parameter',2, 1)
        
        lowByte = calllib('dynamixel','dxl_get_lowbyte', pos_1);
        highByte = calllib('dynamixel','dxl_get_highbyte', pos_1);
        calllib('dynamixel','dxl_set_txpacket_parameter',3, lowByte);
        calllib('dynamixel','dxl_set_txpacket_parameter',4, highByte);
        
        %Speed = 512
        lowByte= calllib('dynamixel','dxl_get_lowbyte', speed/5);
        highByte = calllib('dynamixel','dxl_get_highbyte', speed/5);
        calllib('dynamixel','dxl_set_txpacket_parameter',5, lowByte);
        calllib('dynamixel','dxl_set_txpacket_parameter',6, highByte);
        
        %Parameters for syncwrite dynamixel id = 2
        % id | position | speed
        %ID = 2
        calllib('dynamixel','dxl_set_txpacket_parameter',7, 2)
        
        %Position = 512
        lowByte = calllib('dynamixel','dxl_get_lowbyte', pos_2);
        highByte = calllib('dynamixel','dxl_get_highbyte', pos_2);
        calllib('dynamixel','dxl_set_txpacket_parameter',8, lowByte);
        calllib('dynamixel','dxl_set_txpacket_parameter',9, highByte);
        
        %Speed = 512
        lowByte = calllib('dynamixel','dxl_get_lowbyte', speed);
        highByte = calllib('dynamixel','dxl_get_highbyte', speed);
        calllib('dynamixel','dxl_set_txpacket_parameter',10, lowByte);
        calllib('dynamixel','dxl_set_txpacket_parameter',11, highByte);
        
        %Parameters for syncwrite dynamixel id = 3
        % id | position | speed
        %ID = 3
        calllib('dynamixel','dxl_set_txpacket_parameter',12, 3)
        
        %Position = 512
        lowByte = calllib('dynamixel','dxl_get_lowbyte', pos_3);
        highByte = calllib('dynamixel','dxl_get_highbyte', pos_3);
        calllib('dynamixel','dxl_set_txpacket_parameter',13, lowByte);
        calllib('dynamixel','dxl_set_txpacket_parameter',14, highByte);
        
        %Speed = 512
        lowByte = calllib('dynamixel','dxl_get_lowbyte', speed);
        highByte = calllib('dynamixel','dxl_get_highbyte', speed);
        calllib('dynamixel','dxl_set_txpacket_parameter',15, lowByte);
        calllib('dynamixel','dxl_set_txpacket_parameter',16, highByte);
       
        %transmit
        calllib('dynamixel','dxl_tx_packet');      
        
    else
        disp('Failed to open USB2Dynamixel!');
    end
    calllib('dynamixel','dxl_terminate');
%     unloadlibrary('dynamixel');